# This is a sample Python script.

# Press Shift+F10 to execute it or replace it with your code.
# Press Double Shift to search everywhere for classes, files, tool windows, actions, and settings.
import cv2
import open3d as o3d
import pose
import stream
from camera import project_pano

# from crf import CRF_point


# Use a breakpoint in the code line below to debug your script.

# setting for file path
CLOUD_file = "/home/ivan/Datasets/SRTP-DATA/20210112-CoreSixPano/iScan-Pcd-1 - Cloud.pcd"
SYN_root = "/home/ivan/Datasets/SRTP-DATA/20210112-CoreSixPano/ld-image-cylinder/"
POS_path = "/home/ivan/Datasets/SRTP-DATA/20210112-CoreSixPano/CoreSix.pos"
SYN_file = "ld-image.syn"

# load files
pcd = o3d.io.read_point_cloud(CLOUD_file)
pos = pose.POSFile(POS_path, "")
panoptic = stream.Stream(SYN_root,SYN_file)

# trajectory = o3d.geometry.PointCloud()
# trajectory.points = o3d.utility.Vector3dVector((pos_df[["X", "Y", "H-Ell"]].to_numpy()))
# trajectory.paint_uniform_color([1, 0, 0])

# trans cloud from model to ecef
pcd.transform(pose.rt2homo(*pos.get_model_ecef()))

i = 10
# select the i-th frame
pano, t = panoptic.get_image(i)
orient, CX = pos.get_cam_ecef_by_time(t)
axis_pcd = o3d.geometry.TriangleMesh.create_coordinate_frame(size=5, origin=CX)
axis_pcd.rotate(orient) # rotate axis from world ro cam

# search points within a radius of 30 m
pcd_tree = o3d.geometry.KDTreeFlann(pcd)
[k, idx, _]= pcd_tree.search_radius_vector_3d(CX, 30)
region = pcd.select_by_index(idx)

# trans points from world to camera
region.translate(-CX)
region.rotate(orient.T, center=(0,0,0))

prj = project_pano(region, pano) # get points projection

cv2.namedWindow("pano", cv2.WINDOW_NORMAL)
cv2.namedWindow("pano_prj", cv2.WINDOW_NORMAL)
cv2.waitKey(1)
cv2.imshow("pano",pano)
cv2.imshow("pano_prj",prj)
cv2.waitKey(1)
# show model
o3d.visualization.draw_geometries([pcd, axis_pcd])
cv2.waitKey(0)